/*****************************************************************//**
 * \file   EndToolForcep.cpp
 * \brief  关于Forcep相关操作
 *
 * \author Administrator
 * \date   July 2021
 * \modify by Administrator
 * \date   July 2021

 *********************************************************************/

#include "EndToolForcep.h"

 /**
  * \brief   EndToolBoplarHook类构造函数.
  *
  */
EndToolForcep::EndToolForcep()
{
	bInitDone = true;

	//沿着z轴平移0.011米
	transM1 << 1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, 1, 0.006,
		0, 0, 0, 1;

	//绕y轴逆时针旋转90度
	transM2 << cos(PI / 2), 0, sin(PI / 2), 0,
		0, 1, 0, 0,
		-sin(PI / 2), 0, cos(PI / 2), 0,
		0, 0, 0, 1;

	transMatrix_1 = transM1 * transM2;

	//沿着z轴平移0.36986米
	transM3 << 1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, 1, instrument_length,
		0, 0, 0, 1;

	transMatrix = transM1 * transM2 * transM3;
}

/**
 * \brief   EndToolForcep类析构函数.
 *
 */
EndToolForcep::~EndToolForcep()
{

}

/**
 * \brief   获取手术器械末端的位姿.
 *
 * \param robotend 机械臂末端位姿
 * \return         末端器械位姿
 */
Eigen::Matrix4d EndToolForcep::geToolEndMatrix(Eigen::Matrix4d robotend)
{
	return robotend * transMatrix;
}

/**
 * \brief   通过RCM创口点约束，将主手位姿加持到器械末端后，求得机械臂末端.
 *
 * \param robotend_current  机械臂当前末端位姿
 * \param toolend_next      末端机械下一个位姿
 * \param RCM               RCM点
 * \return                  机械臂下一个位姿
 */
Eigen::Matrix4d EndToolForcep::getRobotNextEnd(Eigen::Matrix4d robotend_current, Eigen::Matrix4d toolend_next, Eigen::Vector3d RCM)
{
	//对应于旋转矩阵的xi，yi，zi
	Eigen::Vector3d toolM1_Xi, toolM1_Yi, toolM1_Zi;
	//toolM1为RCM反解时对应的矩阵（反解求机械臂末端），toolend_current时通过机械臂末端矩阵*转换矩阵后，得到的当前器械末端矩阵。
	Eigen::Matrix4d toolM1, toolend_current;

	//得到的当前器械末端矩阵
	toolend_current = robotend_current * transMatrix;

	//求手术机械在人体外的长度
	instrument_out = instrument_length - (toolend_next.block(0, 3, 3, 1) - RCM).norm();


	//求当前器械末端到RCM，与目标器械末端到RCM点的差值
	distance_oldAndnew = ((RCM - toolend_next.block(0, 3, 3, 1)).norm()) - ((toolend_current.block(0, 3, 3, 1) - RCM).norm());

	//计算满足RCM约束下的目标器械末端的姿态
	toolM1_Zi = (toolend_next.block(0, 3, 3, 1) - RCM) / (RCM - toolend_next.block(0, 3, 3, 1)).norm();
	toolM1_Xi = toolend_next.block(0, 0, 3, 1);

	toolM1_Yi = toolM1_Zi.cross(toolM1_Xi);
	toolM1_Yi = toolM1_Yi / toolM1_Yi.norm();
	toolM1_Xi = toolM1_Yi.cross(toolM1_Zi);

	//将目标姿态组成到反解求机械臂末端的矩阵中
	toolM1.block(0, 0, 3, 1) = toolM1_Xi;
	toolM1.block(0, 1, 3, 1) = toolM1_Yi;
	toolM1.block(0, 2, 3, 1) = toolM1_Zi;

	//将目标位置组成到反解求机械臂末端的矩阵中
	toolM1.block(0, 3, 3, 1) = RCM - toolM1_Zi * (instrument_out);
	toolM1.block(3, 0, 1, 4) << 0, 0, 0, 1;

	return toolM1 * transMatrix_1.inverse();
}

/**
 * \brief   获取机械臂末端手术器械种类.
 *
 * \return  末端器械类型
 */
int EndToolForcep::getEndToolType()
{
	return 6;
}
